#include "sys.h"
#include "taskD.h"
#include "app.h"
//19/18

extern u16 step;
extern int Dcenter_flag1, Dcenter_flag2,  W_flag;
u16 step_D = 0, high_D = 0, mode_D = 0, count_D = 0, count_move = 0, move_flag = 1;
float yjudgeX=77/*76.0*/, yjudgeX2=30, yjudgeX3=70, yjudgeS=40.0;;	//high = 1 down or 0 up
float wlimit1x=7.0, wlimit1s=5.0,  wlimit2xx=25/*25/18*/, wlimit2xs=10, wlimit2s=17;//10
float ylimit1x=50, ylimit1s=50,  ylimit2x=10, ylimit2s=10;
float wtar=45.0;		//closs to fruit
float wlim=6.0; 		//S or X change mode_D 0X, 1S
static int turn = 0;				//0第一趟 1第二趟
float x_err1 = 0.5, x_err2 = 1, w_err = 1;

//串口1为:树莓派/jetson nano
//串口2为:机械臂stm32f103
//串口3为:小车stm32f103		
//串口4为:雷达
//串口5为:蓝牙HC-12
//串口6为:语音模块



void actionD(float x_target, float w_target)		
{
	//int i;
	float x_target2 = 85.0;
	//float y_aver, w_aver;
	static u16 else_count = 0;
	if(move_flag)
	{
		if(count_D <= 1 && mode_D != 2)
		{
			if(!turn)closetowall_D(-1, 2.23, 0.89);
			else closetowall_D(1.25, -1, 0.88);
		}
		else if(count_D >= 2 && mode_D != 1)
		{
			if(!turn)closetowall_D(-1, 1.23, 0.89);
			else closetowall_D(2.25, -1, 0.88);
		}
	}
	move_flag = 1;
	delay_ms(500);
	if(ctf != -1 || (count_D>=2&&mode_D==1) || (count_D<=1&&mode_D==2)) //有熟的
	{
		u5_printf("see ctf:%d, mode_D:%d\r\n", ctf, mode_D);
		u5_printf("O:%d, X:%.2f, Y:%.2f, W:%.2f, H:%.2f, ratio:%.2f\r\n",fruit_rem.object, fruit_rem.X, fruit_rem.Y, fruit_rem.W, fruit_rem.H, fruit_rem.ratio);
		else_count = 0;
		u5_printf("fruit_rem.Y:%.2f\r\n", fruit_rem.Y);
		if(fruit_rem.Y<yjudgeX)high_D = 1;
		else high_D = 0;
		if(!mode_D) //X
		{
			u5_printf("X\r\n");
			ClossToFruitX(x_target, w_target);
		}
		else if(mode_D==1)//right S
		{
			if(count_D>=2)//侧抓朝左direction = 1;
			{
				u5_printf("SS\r\n");
				//direction = 0;
				direction = 1;
				ClossToFruitS(x_target2, w_target);
			}
			else
			{
				u5_printf("SX\r\n");
				//direction = 1;
				ClossToFruitX(x_target, w_target);//直抓
			}
		}
		else //left S
		{
			if(count_D>=2)
			{
				u5_printf("SS\r\n");
				//direction = 0;
				ClossToFruitX(x_target, w_target);//直抓
			}
			else//侧抓朝右direction = 0;
			{
				u5_printf("SX\r\n");
				direction = 0;
				ClossToFruitS(100-x_target2, w_target);
			}
		}
	}
	else //看不到熟的
	{ 
		u5_printf("can't see ctf:%d\r\n", ctf);
		for(else_count=10;else_count>0&&ctf==-1;else_count--)
		{
			u5_printf("else_count:%d\r\n", else_count);
			if(count_D<=1)
				u3_printf("~%c%.2f~%c%.2f\r\n",'Y',0.05,'E',speed_fast),wait_s(),delay_ms(500);			
			else
				u3_printf("~%c%.2f~%c%.2f\r\n",'Y',-0.05,'E',speed_fast),wait_s(),delay_ms(500);
		}
		delay_ms(2000);
		if(else_count == 0&&ctf==-1)
		{
			order(3, 'S'); //让车停下
			step_D = turn==1?6:3;
		}
		else move_flag = 0;
	}
}

void find_D(int high)
{
	order_num(2, 'K', high+2);
	u5_printf("high_mode:%d\r\n", high);
}

void find_DS(int high)
{
	order_num(2, 'K', high+4);
	order_num(2, 'H', 180*direction);
	u5_printf("high_mode:%d\r\n", high);
}

void ClossToFruitS(float x_target, float w_target)
{
	int i=0, count_move=0;
	float speed;
	//坡下
	//调整X
	count_move=0;
	u5_printf("S1 Xmoving\r\n");
	if(mode_D==1)
	{
		if(!turn)closetowall_D(-1, 1.10, 0.89);
		else closetowall_D(2.44, -1, 0.88);
	}
	else if(mode_D==2)
	{
		if(!turn)closetowall_D(-1, 2.44, 0.89);
		else closetowall_D(1.10, -1, 0.88);
	}
	//find_D(1);
	order_num(2, 'H', 180*direction);
	delay_ms(1000);
	wlimit = wlimit2s;
	ylimit = ylimit2s;
	carup();
	//坡上
	order_num(3, 'W',speed_slow*2);
	W_flag = 1;
	for(i=0; (ctf == -1 || !fruit_rem.ripe) && i<15; i++)
	{
		u5_printf("find again\r\n");
		delay_ms(760);
	}
	if(i==15)order(3, 'S'),delay_ms(1000); //让车停下
	
	if(ctf != -1 && fruit_rem.ripe)
	{
		count_move=0;
		u5_printf("S2 Xmoving\r\n");
		u5_printf("O:%d, X:%.2f, Y:%.2f, W:%.2f, H:%.2f, ratio:%.2f\r\n",fruit_rem.object, fruit_rem.X, fruit_rem.Y, fruit_rem.W, fruit_rem.H, fruit_rem.ratio);
		for(i=1;i>-1;i--)
		{
			
			speed = speed_slow*(0.5+i*0);
			if(fruit_rem.X > 45)
			{
				if(direction)
					order_num(3, 'W',-1*speed);		//水果在左边时
				else
					order_num(3, 'W',speed);	//水果在右边时
				while(fruit_rem.X>45+x_err2);
			}
			else
			{
				if(direction)
					order_num(3, 'w',-1*speed);	
				else
					order_num(3, 'w',speed);	
				while(fruit_rem.X<45-x_err2);
			}
			order(3, 'S'); //让车停下
			u5_printf("X:%.2f",fruit_rem.X);
			delay_ms(500);
			if(!i && fabs(fruit_rem.X-45)>5 && count_move<10)
			{
				i=2;
				count_move++;
			}
		}	
		count_move=0;
		u5_printf("fruit_rem.Y:%.2f\r\n", fruit_rem.Y);
		if(fruit_rem.Y<yjudgeS)high_D = 1;
		else high_D = 0;
		find_DS(high_D);
		u5_printf("S2 Wmoving\r\n");
		for(i=1;i>-1;i--)
		{	
			
			speed = speed_slow*(0.5+i*3);
			if(fruit_rem.W>w_target)
			{
				if(direction)
					order_num(3, 'E',speed);		//水果在左边时
				else
					order_num(3, 'Q',speed);	//水果在右边时
				while(fruit_rem.W>w_target+w_err);//u5_printf("Wide:%.2f\r\n",Wide);
			}
			else
			{
				if(direction)
					order_num(3, 'Q',speed);		//水果在左边时
				else
					order_num(3, 'E',speed);	//水果在右边时
				while(fruit_rem.W<w_target-w_err);//u5_printf("Wide:%.2f\r\n",Wide);
			}
			order(3, 'S'); //让车停下
			u5_printf("W:%.2f",fruit_rem.W);
			delay_ms(500);
			if(!i && fabs(fruit_rem.W-w_target)>5 && count_move<10)
			{
				i=2;
				count_move++;
			}
		}		
		step_D = turn?5:2;	
	}
	else
	{
		delay_ms(500);
		cardown();
		step_D = turn?4:1;
	}
	W_flag = 0;
}

void ClossToFruitX(float x_target, float w_target)
{
	int i=0;
	float speed;
	char x_dir = 'E',y_dir = 'W';
	//坡下
	//快速接近
	count_move=0;
	u5_printf("X1 Xmoving\r\n");
	for(i=1;i>-1;i--)
	{
		speed = speed_slow*(1+i*2);
		if(fruit_rem.X > x_target)
		{
			order_num(3, x_dir,-1*speed);	
			while(fruit_rem.X>x_target+x_err1);
		}
		else
		{
			order_num(3, x_dir,speed);		
			while(fruit_rem.X<x_target-x_err1);
		}
		order(3, 'S'); //让车停下
		u5_printf("X:%.2f",fruit_rem.X);
		delay_ms(500);
		if(!i && fabs(fruit_rem.X-x_target)>5 && count_move<10)
		{
			i=2;
			count_move++;
		}
	} 
	u5_printf("fruit_rem.Y:%.2f\r\n", fruit_rem.Y);
	if(fruit_rem.Y<yjudgeX)high_D = 1;
	else high_D = 0;
	find_D(high_D);
	delay_ms(1000);
	wlimit = mode_D?wlimit2xs:wlimit2xx;
	ylimit = ylimit2x;
	carup();
	u5_printf("on stage\r\n");
	//坡上
	delay_ms(1000);
	u5_printf("judge\r\n");
	W_flag = 1;
	for(i=0; (ctf == -1 || !fruit_rem.ripe) && i<5; i++)
	{
		u5_printf("find again\r\n");
		delay_ms(1000);
	}
	u5_printf("pre\r\n");
	if(ctf != -1)
	{
		//慢速调整
		count_move=0;
		u5_printf("X2 Xmoving\r\n");
		for(i=1;i>-1;i--)
		{
			speed = speed_slow*(1+i*2);
			if(fruit_rem.X > x_target)
			{
				order_num(3, x_dir,-1*speed);	
				while(fruit_rem.X>x_target+x_err2);
			}
			else
			{
				order_num(3, x_dir,speed);		
				while(fruit_rem.X<x_target-x_err2);
			}
			order(3, 'S'); //让车停下
			u5_printf("X:%.2f",fruit_rem.X);
			delay_ms(500);
			if(!i && fabs(fruit_rem.X-x_target)>5 && count_move<10)
			{
				i=2;
				count_move++;
			}
		}
		W_flag = 0;
		count_move=0;
		u5_printf("fruit_rem.Y:%.2f\r\n", fruit_rem.Y);
		if(fruit_rem.Y<yjudgeX2&&!high_D)high_D = 1;
		else if(high_D&&fruit_rem.Y>yjudgeX3)high_D = 0;
		find_D(high_D);
		u5_printf("X2 Wmoving\r\n");
		for(i=1;i>-1;i--)
		{
			speed = speed_slow*(1+i*0);
			if(fruit_rem.W > w_target)
			{
				order_num(3, y_dir,-1*speed);	
				while(fruit_rem.W>w_target+w_err);//u5_printf("W:%.2f\r\n",fruit_rem.W);
			}
			else
			{
				order_num(3, y_dir,speed);		
				while(fruit_rem.W<w_target-w_err);//u5_printf("W:%.2f\r\n",fruit_rem.W);
			}
			order(3, 'S'); //让车停下
			u5_printf("W:%.2f",fruit_rem.W);
			delay_ms(500);
			if(!i && fabs(fruit_rem.W-w_target)>5 && count_move<10)
			{
				i=2;
				count_move++;
			}
		}
		step_D = turn?5:2;
	}
	else
	{	
		delay_ms(500);
		cardown();
		step_D = turn?4:1;
	}
	W_flag = 0;
}

void GrapFruit_D(uint8_t high)
{
	#define h 0.16
	#define pepper_H 52 //45完整的苹果的H
	float High[]={43.5,36},fruit_Y[]={55.5,67};		//D1对应的中心辣椒的高与离地高度 fruit_Y[1]画面正中心
	float d_Y,real_Y;
	uint8_t high_num;
	d_Y = (fruit_rem.H-pepper_H)/2;
	if(fruit_rem.Y>50) real_Y = fruit_rem.Y-d_Y;
	else real_Y = fruit_rem.Y+d_Y;
	//估算实际高度并转换为抓取高度
	fruit_rem.high = (real_Y-fruit_Y[high])*h+High[high];   
	high_num = HighToNum(fruit_rem.high);
	if(high_num==7) ClossToFruitX(50, 37);
	if(fruit_rem.ripe)
	{
		u5_printf("num:%d Y:%f W:%f real_Y:%f high:%f H:%f\r\n",high_num,fruit_rem.Y,fruit_rem.W,real_Y,fruit_rem.high,fruit_rem.H);
		order_num(2,'G',high_num); 
	}
}


void cardown()
{
	order_num(3, 'w', speed_fast*2);	
	while(carstate == 1);
	delay_ms(20);
	while(carstate != 1);
	order(3, 'S'); //让车停下
	delay_ms(500);
	u3_printf("~%c%.2f~%c%.2f\r\n",'X',-0.28,'W',speed_fast*1.5),wait_s();			//向后平移
	order(3, 'S'); //让车停下
}

void carup()
{
	order_num(3, 'W', speed_fast*2);	
	while(carstate == 1);
	delay_ms(20);
	while(carstate != 1);
	order(3, 'S'); //让车停下
	delay_ms(500);
	//while(1)u5_printf("W:%.2f\r\n", fruit_rem.W);
	if(fruit_rem.W<40.0)u5_printf("carup fruit_rem.W:%.2f\r\n", fruit_rem.W),u3_printf("~%c%.2f~%c%.2f\r\n",'X',0.14,'W',speed_fast),wait_s();			//向前平移
	order(3, 'S'); //让车停下
}

void closetowall_D(float L_tar, float R_tar, float B_tar)
{
	int i;
	float speed;
	char x_dir = 'E';
	char y_dir = 'W';
	speed = (speed_fast*(1+i*1.5));
	if(L_tar>=0)
	{
		count_move = 0;
		for(i=1;i>-1;i--)
		{
			u5_printf("Lmoving:%.2f\r\n", Leida_Distance[0]);
			if(Leida_Distance[0] > L_tar)
			{
				order_num(3, x_dir,-1*speed);	
				while(Leida_Distance[0]>L_tar);
			}
			else
			{
				order_num(3, x_dir,speed);		
				while(Leida_Distance[0]<L_tar);
			}
			order(3, 'S'); //让车停下
			u5_printf("L:%.2f",Leida_Distance[0]);
			delay_ms(100);
			if(!i && fabs(Leida_Distance[0]-L_tar)>0.02 && count_move<10)
			{
				i=2;
				count_move++;
			}
		} 
	}
	if(R_tar>=0)
	{
		count_move = 0;
		for(i=1;i>-1;i--)
		{
			u5_printf("Rmoving:%.2f\r\n", Leida_Distance[1]);
			if(Leida_Distance[1] > R_tar)
			{
				order_num(3, x_dir,speed);	
				while(Leida_Distance[1]>R_tar);
			}
			else
			{
				order_num(3, x_dir,-1*speed);		
				while(Leida_Distance[1]<R_tar);
			}
			order(3, 'S'); //让车停下
			u5_printf("R:%.2f",Leida_Distance[1]);
			delay_ms(100);
			if(!i && fabs(Leida_Distance[1]-R_tar)>0.02 && count_move<10)
			{
				i=2;
				count_move++;
			}
		} 
	}
	if(B_tar>=0)
	{
		count_move = 0;
		for(i=1;i>-1;i--)
		{
			u5_printf("Bmoving:%.2f\r\n", Leida_Distance[2]);
			if(Leida_Distance[2] > B_tar)
			{
				order_num(3, y_dir,-1*speed);	
				while(Leida_Distance[2]>B_tar);
			}
			else
			{
				order_num(3, y_dir,speed);		
				while(Leida_Distance[2]<B_tar);
			}
			order(3, 'S'); //让车停下
			u5_printf("B:%.2f",Leida_Distance[2]);
			delay_ms(100);
			if(!i && fabs(Leida_Distance[2]-B_tar)>0.02 && count_move<10)
			{
				i=2;
				count_move++;
			}
		} 
	}
}

void carcenter()
{
//	int i;
//	char x_dir = 'E';
//	char y_dir = 'W';
//	float speed;
	float L_tar;
	float R_tar;
	float B_tar;
	if(!turn)//第一趟
	{
		L_tar = -1;//7.54
		R_tar = 1.73;
		B_tar = 0.89;
	}
	else if(turn == 1)
	{
		L_tar = 1.75;
		R_tar = -1;//7.54;
		B_tar = 0.88;
	}
	closetowall_D(L_tar, R_tar, B_tar);
}

void judgeXS()
{
	int count1 = 0, count2 = 0, i = 0;
	for(i=0;i<10;i++)
	{
		if(Dcenter_flag1==1)count1++, count2--;
		if(Dcenter_flag2==1)count2++, count1--;
		if(!Dcenter_flag1&&!Dcenter_flag2) count1--, count2--;
		u5_printf("judgeXS count1:%d, count2:%d\r\n", count1, count2);
		delay_ms(500);
	}
	if(count1>count2&&count1>=-2)mode_D = 1;//right S
	else if(count1<count2&&count2>=-2)mode_D = 2;//left S
	else mode_D = 0;//X
	u5_printf("mode_D:%d\r\n", mode_D);
	step_D++;
}

void D_2ToD_9()
{
	float L_tar = -1;//6.85;
	float R_tar = 2.44;//2.39
	float B_tar = 0.89;
	closetowall_D(L_tar, R_tar, B_tar);
	order_num(3, 'W', speed_fast*2);	
	while(carstate == 1);
	while(carstate != 1);
	delay_ms(2000);
	while(carstate == 1);
	while(carstate != 1);
	delay_ms(1800);
	order(3, 'S'); //让车停下
	//u3_printf("~%c%.2f~%c%.2f\r\n",'X',0.25,'W',speed_fast),wait_s();			//向前平移
	Turn_180();
	carcenter();
	step_D++;
}
	
void TaskD(void)
{
  char num[3]={"000"};
	switch(step_D)
	{
		case 0:
		{
			//while(1)u5_printf("X:%.2f, Y:%.2f, W:%.2f, H:%.2f, ratio:%.2f\r\n",fruit_rem.X, fruit_rem.Y, fruit_rem.W, fruit_rem.H, fruit_rem.ratio);
			u5_printf("Dcase0\r\n");
			wlimit = mode_D?wlimit1s:wlimit1x;
			ylimit = mode_D?ylimit1s:ylimit1x;
			high_D = 0;
			mode_D = 0;
			count_D = 0;
			turn = 0;
			order_num(2, 'H', 90);
			find_D(high_D);
			carcenter();
			Dcenter_flag1 = 0;
			Dcenter_flag2 = 0;
			judgeXS();
			//while(1);
			break;
		}
		case 1:
		{
			u5_printf("Dcase1\r\n");
			wlimit = mode_D?wlimit1s:wlimit1x;
			ylimit = mode_D?ylimit1s:ylimit1x;
			high_D = 0;
			order_num(2, 'H', 90);
			find_D(high_D);
			actionD(45, wtar);
			break;
		}
		case 2:
		{
			u5_printf("Dcase2\r\n");
			u5_printf("high_D:%d\r\n", high_D);
			GrapFruit_D(high_D);
			delay_ms(500);
			cardown();
			SWLED_Flag = 0;	
			order_num(2,'g',1);
			SWLED_Flag = 1;
			SYN_FrameInfo(0,intToString(GrapFruitNum,num),0);
			count_D++;
			if(count_D>=4)step_D++;
			else step_D = 1;
			break;
		}
		case 3:
		{
			u5_printf("Dcase3\r\n");
			//while(1);
			wlimit = mode_D?wlimit1s:wlimit1x;
			ylimit = mode_D?ylimit1s:ylimit1x;
			high_D = 0;
			count_D = 0;
			turn = 1;
			order_num(2, 'H', 90);
			find_D(high_D);
			D_2ToD_9();
			break;
		}
		case 4:
		{
			u5_printf("Dcase4\r\n");
			wlimit = mode_D?wlimit1s:wlimit1x;
			ylimit = mode_D?ylimit1s:ylimit1x;
			high_D = 0;
			order_num(2, 'H', 90);
			find_D(high_D);
			actionD(45, wtar);
			break;
		}
		case 5:
		{
			u5_printf("Dcase5\r\n");
			u5_printf("high_D:%d\r\n", high_D);
			GrapFruit_D(high_D);
			cardown();
			SWLED_Flag = 0;	
			order_num(2,'g',1);
			SYN_FrameInfo(0,intToString(GrapFruitNum,num),0);
			SWLED_Flag = 1;
			count_D++;
			if(count_D>=4)step_D++;
			else step_D = 4;
			break;
		}
		case 6:
		{
			step++;
			break;
		}
	}
}

